/*
* @File Name prm.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\prm\prm.c
* @Author: Ruige_Lee
* @Date:   2019-07-27 14:40:45
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-07-30 20:27:14
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/
/*
* @File Name prm.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\prm\prm.c
* @Author: 29505
* @Date:   2019-07-23 21:39:07
* @Last Modified by:   29505
* @Last Modified time: 2019-07-23 22:18:28
* @Email: 295054118@whut.edu.cn
*/

#include <stdint.h>
#include <string.h>

// 产生 X-11 Y-17 Z-13 个节点 （2431）
// X:100~350 Y:-200~200 Z:100~400
// 编号 X(0-10) Y(0-16) Z(0-12)
// 结点编号 Z*187 + Y*11 + X

// 产生11*17*12 + 11*16*13 + 10*17*13 = 6742条edge
// edge 编号 (0-6741)
// 建立二维数组从pose映射到edge
// X方向edge 0 + 10 * 17 * 13
// Y方向edge 2210 + 11 * 16 * 13
// Z方向edge 4498 + 11 * 17 * 12

// 轨迹和规划周期应该有流水线
// 规划时，还有部分轨迹没运动完，但却是绝对安全的

/*
	输入当前位置，目标位置, 当前长轨迹

	当前轨迹终点与实际目标不合（轨迹点表），goto 规划


	预判当前长轨迹干涉（实际干涉和预期干涉），为真，goto部分规划

	
	输出理想长轨迹
*/


// coding of pose: 0~2430
// coding of edge: 0~6741


/**
 * [poseIndex2Coordinate description] 功能函数，由已知pose索引输出实际索引。单位： 1
 * 										z_index*187 + y_index*11 + x_index = Pose
 * @return [description] x_index,y_index,z_index;
 */
int32_t poseNum2Index(uint16_t poseNum, uint8_t xyz_index[3])
{
	xyz_index[2] = poseNum / 187;
	xyz_index[1] = (poseNum % 187) / 11;
	xyz_index[0] = poseNum % 11;

	return 0;
}

/**
 * [poseIndex2Coordinate description] 功能函数，根据已知的poseIndex返回对应坐标
 * @param  poseNum [description]
 * @return         [description]
 */
static int32_t poseIndex2Coordinate(uint8_t poseNum, int16_t xyz_mm[3])
{

	xyz_mm[0] =  100 + poseNum * 25;
	xyz_mm[1] = -200 + poseNum * 25;
	xyz_mm[2] =  100 + poseNum * 25;

	return 0;
}

/**
 * [edgeIndex2edgeNum description] edge 有指向问题，建议6条边同步传入
 * @return [description]
 */
static uint32_t edgeIndex2edgeNum(uint8_t edgeIndex[6][3], uint16_t edgeNum[6])
{
	for ( uint8_t i = 0; i < 6; i ++ )
	{
		edgeNum[i] = 65535;
	}

	//x_dir edge_x:0~9 edge_y:0~16 edge_z:0~12  0~2209
	if (edgeIndex[0][0] != 255)
	{
		edgeNum[0] = edgeIndex[0][2] * 170 + edgeIndex[0][1] * 10 + edgeIndex[0][0];
	}

	if (edgeIndex[1][0] != 255)
	{
		edgeNum[1] = edgeIndex[1][2] * 170 + edgeIndex[1][1] * 10 + edgeIndex[1][0];
	}

	//y_dir edge_x:0~10 edge_y:0~15 edge_z:0~12 = 2210 + 0 ~ 2287 =   2210~4497
	if (edgeIndex[2][0] != 255)
	{
		edgeNum[2] = 2210 + edgeIndex[2][2] * 176 + edgeIndex[2][1] * 11 + edgeIndex[2][0];
	}

	if (edgeIndex[3][0] != 255)
	{
		edgeNum[3] = 2210 + edgeIndex[3][2] * 176 + edgeIndex[3][1] * 11 + edgeIndex[3][0];
	}

	//z_dir edge_x:0~10 edge_y:0~16 edge_z:0~11 = 4498 + 0 ~ 2243 =   4498~6741
	if (edgeIndex[4][0] != 255)
	{
		edgeNum[4] = 4498 + edgeIndex[4][2] * 187 + edgeIndex[4][1] * 11 + edgeIndex[4][0];
	}

	if (edgeIndex[5][0] != 255)
	{
		edgeNum[5] = 4498 + edgeIndex[5][2] * 187 + edgeIndex[5][1] * 11 + edgeIndex[5][0];
	}
	return 0;
}


/**
 * [poseActivateEdge description] 根据Pose Num 返回可供激活的 edgeNum
 * @return [description]
 */
uint32_t poseNumActivateNextPoseNumEdgeNum( uint16_t poseNum, uint16_t nextPoseNum[6], uint16_t edgeNum[6])
{

	// 三个指向，相邻，[XYZ]
	uint8_t edgeIndex[6][3];

	edgeNum[0] = 65535;
	edgeNum[1] = 65535;
	edgeNum[2] = 65535;
	edgeNum[3] = 65535;
	edgeNum[4] = 65535;
	edgeNum[5] = 65535;

	uint8_t xyz_index[3];
	poseNum2Index(poseNum,xyz_index);

	// edge 0
	if ( xyz_index[0] != 10 )
	{
		edgeIndex[0][0] = xyz_index[0];
		edgeIndex[0][1] = xyz_index[1];
		edgeIndex[0][2] = xyz_index[2];
	}
	else
	{
		edgeIndex[0][0] = 255;
		edgeIndex[0][1] = 255;
		edgeIndex[0][2] = 255;
	}
	// edge 1
	if ( xyz_index[0] != 0 )
	{
		edgeIndex[1][0] = xyz_index[0] - 1;
		edgeIndex[1][1] = xyz_index[1];
		edgeIndex[1][2] = xyz_index[2];
	}
	else
	{
		edgeIndex[1][0] = 255;
		edgeIndex[1][1] = 255;
		edgeIndex[1][2] = 255;
	}
	// edge 2
	if ( xyz_index[1] != 16 )
	{
		edgeIndex[2][0] = xyz_index[0];
		edgeIndex[2][1] = xyz_index[1];
		edgeIndex[2][2] = xyz_index[2];
	}
	else
	{
		edgeIndex[2][0] = 255;
		edgeIndex[2][1] = 255;
		edgeIndex[2][2] = 255;
	}
	//edge 3
	if ( xyz_index[1] != 0 )
	{
		edgeIndex[3][0] = xyz_index[0];
		edgeIndex[3][1] = xyz_index[1] - 1;
		edgeIndex[3][2] = xyz_index[2];
	}
	else
	{
		edgeIndex[3][0] = 255;
		edgeIndex[3][1] = 255;
		edgeIndex[3][2] = 255;
	}
	//edge 4
	if ( xyz_index[2] != 12 )
	{
		edgeIndex[4][0] = xyz_index[0];
		edgeIndex[4][1] = xyz_index[1];
		edgeIndex[4][2] = xyz_index[2];
	}
	else
	{
		edgeIndex[4][0] = 255;
		edgeIndex[4][1] = 255;
		edgeIndex[4][2] = 255;
	}
	//edge 5
	if ( xyz_index[2] != 0 )
	{
		edgeIndex[5][0] = xyz_index[0];
		edgeIndex[5][1] = xyz_index[1];
		edgeIndex[5][2] = xyz_index[2] - 1;
	}
	else
	{
		edgeIndex[5][0] = 255;
		edgeIndex[5][1] = 255;
		edgeIndex[5][2] = 255;
	}

	edgeIndex2edgeNum(edgeIndex,edgeNum);

///////////////////////////////////////////////////////
	if ( nextPoseNum == NULL )
		return 0;

	if ( xyz_index[0] != 10 )
		nextPoseNum[0] = poseNum + 1;
	else
		nextPoseNum[0] = 65535;

	if ( xyz_index[0] != 0 )
		nextPoseNum[1] = poseNum - 1;
	else
		nextPoseNum[1] = 65535;

	if ( xyz_index[1] != 16 )
		nextPoseNum[2] = poseNum + 11;
	else
		nextPoseNum[2] = 65535;

	if ( xyz_index[1] != 0 )
		nextPoseNum[3] = poseNum - 11;
	else
		nextPoseNum[3] = 65535;

	if ( xyz_index[2] != 12 )
		nextPoseNum[4] = poseNum + 187;
	else
		nextPoseNum[4] = 65535;

	if ( xyz_index[2] != 0 )
		nextPoseNum[5] = poseNum - 187;
	else
		nextPoseNum[5] = 65535;


	return 0;
}


///**
// * [prm_collideDetection description] 调用PL，快速读回碰撞数据,建议不写了，直接寻址到位
// * @return [description]
// */
//static uint32_t prm_collideDetection()
//{
//	return 0;
//}
//
//uint32_t prm_excute()
//{
//	return 0;
//}
//
//
///*
//	A* 算法展开，部分回读PL数据
//*/
//uint32_t prm_checkPonit_Create( int16_t startPose, int16_t destPose )
//{
//	uint32_t edgeState[8192] = {0};
//
//	//colide result
//	cl_read( uint8_t regPart );
//
//}
